
#include "Precompiled.h"

#include "Matrix.h"
namespace OptixDotNet {
namespace Math
{
	void Matrix4x4::Multiply( Matrix4x4% left, Matrix4x4% right, [Out] Matrix4x4% result )
	{
		Matrix4x4 transpose;
		Matrix4x4::Transpose( right, transpose );

		result.X.Set( left.X.Dot( transpose.X ), left.X.Dot( transpose.Y ), left.X.Dot( transpose.Z ), left.X.Dot( transpose.W ) );
		result.Y.Set( left.Y.Dot( transpose.X ), left.Y.Dot( transpose.Y ), left.Y.Dot( transpose.Z ), left.Y.Dot( transpose.W ) );
		result.Z.Set( left.Z.Dot( transpose.X ), left.Z.Dot( transpose.Y ), left.Z.Dot( transpose.Z ), left.Z.Dot( transpose.W ) );
		result.W.Set( left.W.Dot( transpose.X ), left.W.Dot( transpose.Y ), left.W.Dot( transpose.Z ), left.W.Dot( transpose.W ) );
	}

	Matrix4x4 Matrix4x4::Multiply( Matrix4x4% left, Matrix4x4% right )
	{
		Matrix4x4 transpose;
		Matrix4x4::Transpose( right, transpose );

		Matrix4x4 result;
		result.X.Set( left.X.Dot( transpose.X ), left.X.Dot( transpose.Y ), left.X.Dot( transpose.Z ), left.X.Dot( transpose.W ) );
		result.Y.Set( left.Y.Dot( transpose.X ), left.Y.Dot( transpose.Y ), left.Y.Dot( transpose.Z ), left.Y.Dot( transpose.W ) );
		result.Z.Set( left.Z.Dot( transpose.X ), left.Z.Dot( transpose.Y ), left.Z.Dot( transpose.Z ), left.Z.Dot( transpose.W ) );
		result.W.Set( left.W.Dot( transpose.X ), left.W.Dot( transpose.Y ), left.W.Dot( transpose.Z ), left.W.Dot( transpose.W ) );

		return result;
	}

	void Matrix4x4::Multiply( Matrix4x4 right )
	{
		Matrix4x4 transpose;
		Matrix4x4::Transpose( right, transpose );

		X.Set( X.Dot( transpose.X ), X.Dot( transpose.Y ), X.Dot( transpose.Z ), X.Dot( transpose.W ) );
		Y.Set( Y.Dot( transpose.X ), Y.Dot( transpose.Y ), Y.Dot( transpose.Z ), Y.Dot( transpose.W ) );
		Z.Set( Z.Dot( transpose.X ), Z.Dot( transpose.Y ), Z.Dot( transpose.Z ), Z.Dot( transpose.W ) );
		W.Set( W.Dot( transpose.X ), W.Dot( transpose.Y ), W.Dot( transpose.Z ), W.Dot( transpose.W ) );
	}

	float Matrix4x4::Determinant()
	{
		float det =
		  X.X*Y.Y*Z.Z*W.W-
		  X.X*Y.Y*W.Z*Z.W+X.X*Y.Z*Z.W*W.Y-
		  X.X*Y.Z*Z.Y*W.W+X.X*Y.W*Z.Y*W.Z-
		  X.X*Y.W*Z.Z*W.Y-X.Y*Y.X*Z.Z*W.W+X.Y*Y.X*W.Z*Z.W-
		  X.Y*Y.Z*Z.W*W.X+X.Y*Y.Z*Z.X*W.W-
		  X.Y*Y.W*Z.X*W.Z+X.Y*Y.W*Z.Z*W.X+X.Z*Y.X*Z.Y*W.W-
		  X.Z*Y.X*Z.W*W.Y+X.Z*Y.Y*Z.W*W.X-
		  X.Z*Y.Y*Z.X*W.W+X.Z*Y.W*Z.X*W.Y-
		  X.Z*Y.W*Z.Y*W.X-
		  X.W*Y.X*Z.Y*W.Z+X.W*Y.X*Z.Z*W.Y-
		  X.W*Y.Y*Z.Z*W.X+X.W*Y.Y*Z.X*W.Z-
		  X.W*Y.Z*Z.X*W.Y+X.W*Y.Z*Z.Y*W.X;

		return det;
	}

	void Matrix4x4::Inverse( Matrix4x4% matrix, [Out] Matrix4x4% inverse )
	{
		const float det = 1.0f / matrix.Determinant();

		inverse = Identity;

		inverse.X.X  = det * (matrix.Y.Y * (matrix.Z.Z * matrix.W.W - matrix.Z.W * matrix.W.Z) + matrix.Y.Z * (matrix.Z.W * matrix.W.Y - matrix.Z.Y * matrix.W.W) + matrix.Y.W * (matrix.Z.Y * matrix.W.Z - matrix.Z.Z * matrix.W.Y));
		inverse.X.Y  = det * (matrix.Z.Y * (matrix.X.Z * matrix.W.W - matrix.X.W * matrix.W.Z) + matrix.Z.Z * (matrix.X.W * matrix.W.Y - matrix.X.Y * matrix.W.W) + matrix.Z.W * (matrix.X.Y * matrix.W.Z - matrix.X.Z * matrix.W.Y));
		inverse.X.Z  = det * (matrix.W.Y * (matrix.X.Z * matrix.Y.W - matrix.X.W * matrix.Y.Z) + matrix.W.Z * (matrix.X.W * matrix.Y.Y - matrix.X.Y * matrix.Y.W) + matrix.W.W * (matrix.X.Y * matrix.Y.Z - matrix.X.Z * matrix.Y.Y));
		inverse.X.W = det * (matrix.X.Y * (matrix.Y.W * matrix.Z.Z - matrix.Y.Z * matrix.Z.W) + matrix.X.Z * (matrix.Y.Y * matrix.Z.W - matrix.Y.W * matrix.Z.Y) + matrix.X.W * (matrix.Y.Z * matrix.Z.Y - matrix.Y.Y * matrix.Z.Z));
		inverse.Y.X  = det * (matrix.Y.Z * (matrix.Z.X * matrix.W.W - matrix.Z.W * matrix.W.X) + matrix.Y.W * (matrix.Z.Z * matrix.W.X - matrix.Z.X * matrix.W.Z) + matrix.Y.X * (matrix.Z.W * matrix.W.Z - matrix.Z.Z * matrix.W.W));
		inverse.Y.Y  = det * (matrix.Z.Z * (matrix.X.X * matrix.W.W - matrix.X.W * matrix.W.X) + matrix.Z.W * (matrix.X.Z * matrix.W.X - matrix.X.X * matrix.W.Z) + matrix.Z.X * (matrix.X.W * matrix.W.Z - matrix.X.Z * matrix.W.W));
		inverse.Y.Z  = det * (matrix.W.Z * (matrix.X.X * matrix.Y.W - matrix.X.W * matrix.Y.X) + matrix.W.W * (matrix.X.Z * matrix.Y.X - matrix.X.X * matrix.Y.Z) + matrix.W.X * (matrix.X.W * matrix.Y.Z - matrix.X.Z * matrix.Y.W));
		inverse.Y.W = det * (matrix.X.Z * (matrix.Y.W * matrix.Z.X - matrix.Y.X * matrix.Z.W) + matrix.X.W * (matrix.Y.X * matrix.Z.Z - matrix.Y.Z * matrix.Z.X) + matrix.X.X * (matrix.Y.Z * matrix.Z.W - matrix.Y.W * matrix.Z.Z));
		inverse.Z.X  = det * (matrix.Y.W * (matrix.Z.X * matrix.W.Y - matrix.Z.Y * matrix.W.X) + matrix.Y.X * (matrix.Z.Y * matrix.W.W - matrix.Z.W * matrix.W.Y) + matrix.Y.Y * (matrix.Z.W * matrix.W.X - matrix.Z.X * matrix.W.W));
		inverse.Z.Y  = det * (matrix.Z.W * (matrix.X.X * matrix.W.Y - matrix.X.Y * matrix.W.X) + matrix.Z.X * (matrix.X.Y * matrix.W.W - matrix.X.W * matrix.W.Y) + matrix.Z.Y * (matrix.X.W * matrix.W.X - matrix.X.X * matrix.W.W));
		inverse.Z.Z = det * (matrix.W.W * (matrix.X.X * matrix.Y.Y - matrix.X.Y * matrix.Y.X) + matrix.W.X * (matrix.X.Y * matrix.Y.W - matrix.X.W * matrix.Y.Y) + matrix.W.Y * (matrix.X.W * matrix.Y.X - matrix.X.X * matrix.Y.W));
		inverse.Z.W = det * (matrix.X.W * (matrix.Y.Y * matrix.Z.X - matrix.Y.X * matrix.Z.Y) + matrix.X.X * (matrix.Y.W * matrix.Z.Y - matrix.Y.Y * matrix.Z.W) + matrix.X.Y * (matrix.Y.X * matrix.Z.W - matrix.Y.W * matrix.Z.X));
		inverse.W.X  = det * (matrix.Y.X * (matrix.Z.Z * matrix.W.Y - matrix.Z.Y * matrix.W.Z) + matrix.Y.Y * (matrix.Z.X * matrix.W.Z - matrix.Z.Z * matrix.W.X) + matrix.Y.Z * (matrix.Z.Y * matrix.W.X - matrix.Z.X * matrix.W.Y));
		inverse.W.Y  = det * (matrix.Z.X * (matrix.X.Z * matrix.W.Y - matrix.X.Y * matrix.W.Z) + matrix.Z.Y * (matrix.X.X * matrix.W.Z - matrix.X.Z * matrix.W.X) + matrix.Z.Z * (matrix.X.Y * matrix.W.X - matrix.X.X * matrix.W.Y));
		inverse.W.Z = det * (matrix.W.X * (matrix.X.Z * matrix.Y.Y - matrix.X.Y * matrix.Y.Z) + matrix.W.Y * (matrix.X.X * matrix.Y.Z - matrix.X.Z * matrix.Y.X) + matrix.W.Z * (matrix.X.Y * matrix.Y.X - matrix.X.X * matrix.Y.Y));
		inverse.W.W = det * (matrix.X.X * (matrix.Y.Y * matrix.Z.Z - matrix.Y.Z * matrix.Z.Y) + matrix.X.Y * (matrix.Y.Z * matrix.Z.X - matrix.Y.X * matrix.Z.Z) + matrix.X.Z * (matrix.Y.X * matrix.Z.Y - matrix.Y.Y * matrix.Z.X));
	}

	Matrix4x4 Matrix4x4::Inverse( Matrix4x4% matrix )
	{
		const float det = 1.0f / matrix.Determinant();

		Matrix4x4 inverse = Identity;

		inverse.X.X  = det * (matrix.Y.Y * (matrix.Z.Z * matrix.W.W - matrix.Z.W * matrix.W.Z) + matrix.Y.Z * (matrix.Z.W * matrix.W.Y - matrix.Z.Y * matrix.W.W) + matrix.Y.W * (matrix.Z.Y * matrix.W.Z - matrix.Z.Z * matrix.W.Y));
		inverse.X.Y  = det * (matrix.Z.Y * (matrix.X.Z * matrix.W.W - matrix.X.W * matrix.W.Z) + matrix.Z.Z * (matrix.X.W * matrix.W.Y - matrix.X.Y * matrix.W.W) + matrix.Z.W * (matrix.X.Y * matrix.W.Z - matrix.X.Z * matrix.W.Y));
		inverse.X.Z  = det * (matrix.W.Y * (matrix.X.Z * matrix.Y.W - matrix.X.W * matrix.Y.Z) + matrix.W.Z * (matrix.X.W * matrix.Y.Y - matrix.X.Y * matrix.Y.W) + matrix.W.W * (matrix.X.Y * matrix.Y.Z - matrix.X.Z * matrix.Y.Y));
		inverse.X.W = det * (matrix.X.Y * (matrix.Y.W * matrix.Z.Z - matrix.Y.Z * matrix.Z.W) + matrix.X.Z * (matrix.Y.Y * matrix.Z.W - matrix.Y.W * matrix.Z.Y) + matrix.X.W * (matrix.Y.Z * matrix.Z.Y - matrix.Y.Y * matrix.Z.Z));
		inverse.Y.X  = det * (matrix.Y.Z * (matrix.Z.X * matrix.W.W - matrix.Z.W * matrix.W.X) + matrix.Y.W * (matrix.Z.Z * matrix.W.X - matrix.Z.X * matrix.W.Z) + matrix.Y.X * (matrix.Z.W * matrix.W.Z - matrix.Z.Z * matrix.W.W));
		inverse.Y.Y  = det * (matrix.Z.Z * (matrix.X.X * matrix.W.W - matrix.X.W * matrix.W.X) + matrix.Z.W * (matrix.X.Z * matrix.W.X - matrix.X.X * matrix.W.Z) + matrix.Z.X * (matrix.X.W * matrix.W.Z - matrix.X.Z * matrix.W.W));
		inverse.Y.Z  = det * (matrix.W.Z * (matrix.X.X * matrix.Y.W - matrix.X.W * matrix.Y.X) + matrix.W.W * (matrix.X.Z * matrix.Y.X - matrix.X.X * matrix.Y.Z) + matrix.W.X * (matrix.X.W * matrix.Y.Z - matrix.X.Z * matrix.Y.W));
		inverse.Y.W = det * (matrix.X.Z * (matrix.Y.W * matrix.Z.X - matrix.Y.X * matrix.Z.W) + matrix.X.W * (matrix.Y.X * matrix.Z.Z - matrix.Y.Z * matrix.Z.X) + matrix.X.X * (matrix.Y.Z * matrix.Z.W - matrix.Y.W * matrix.Z.Z));
		inverse.Z.X  = det * (matrix.Y.W * (matrix.Z.X * matrix.W.Y - matrix.Z.Y * matrix.W.X) + matrix.Y.X * (matrix.Z.Y * matrix.W.W - matrix.Z.W * matrix.W.Y) + matrix.Y.Y * (matrix.Z.W * matrix.W.X - matrix.Z.X * matrix.W.W));
		inverse.Z.Y  = det * (matrix.Z.W * (matrix.X.X * matrix.W.Y - matrix.X.Y * matrix.W.X) + matrix.Z.X * (matrix.X.Y * matrix.W.W - matrix.X.W * matrix.W.Y) + matrix.Z.Y * (matrix.X.W * matrix.W.X - matrix.X.X * matrix.W.W));
		inverse.Z.Z = det * (matrix.W.W * (matrix.X.X * matrix.Y.Y - matrix.X.Y * matrix.Y.X) + matrix.W.X * (matrix.X.Y * matrix.Y.W - matrix.X.W * matrix.Y.Y) + matrix.W.Y * (matrix.X.W * matrix.Y.X - matrix.X.X * matrix.Y.W));
		inverse.Z.W = det * (matrix.X.W * (matrix.Y.Y * matrix.Z.X - matrix.Y.X * matrix.Z.Y) + matrix.X.X * (matrix.Y.W * matrix.Z.Y - matrix.Y.Y * matrix.Z.W) + matrix.X.Y * (matrix.Y.X * matrix.Z.W - matrix.Y.W * matrix.Z.X));
		inverse.W.X  = det * (matrix.Y.X * (matrix.Z.Z * matrix.W.Y - matrix.Z.Y * matrix.W.Z) + matrix.Y.Y * (matrix.Z.X * matrix.W.Z - matrix.Z.Z * matrix.W.X) + matrix.Y.Z * (matrix.Z.Y * matrix.W.X - matrix.Z.X * matrix.W.Y));
		inverse.W.Y  = det * (matrix.Z.X * (matrix.X.Z * matrix.W.Y - matrix.X.Y * matrix.W.Z) + matrix.Z.Y * (matrix.X.X * matrix.W.Z - matrix.X.Z * matrix.W.X) + matrix.Z.Z * (matrix.X.Y * matrix.W.X - matrix.X.X * matrix.W.Y));
		inverse.W.Z = det * (matrix.W.X * (matrix.X.Z * matrix.Y.Y - matrix.X.Y * matrix.Y.Z) + matrix.W.Y * (matrix.X.X * matrix.Y.Z - matrix.X.Z * matrix.Y.X) + matrix.W.Z * (matrix.X.Y * matrix.Y.X - matrix.X.X * matrix.Y.Y));
		inverse.W.W = det * (matrix.X.X * (matrix.Y.Y * matrix.Z.Z - matrix.Y.Z * matrix.Z.Y) + matrix.X.Y * (matrix.Y.Z * matrix.Z.X - matrix.Y.X * matrix.Z.Z) + matrix.X.Z * (matrix.Y.X * matrix.Z.Y - matrix.Y.Y * matrix.Z.X));

		return inverse;
	}

	void Matrix4x4::Invert()
	{
		*this = Matrix4x4::Inverse( *this );
	}

	void Matrix4x4::Transpose( Matrix4x4% matrix, [Out] Matrix4x4% transpose )
	{
		transpose.X.Set( matrix.X.X, matrix.Y.X, matrix.Z.X, matrix.W.X );
		transpose.Y.Set( matrix.X.Y, matrix.Y.Y, matrix.Z.Y, matrix.W.Y );
		transpose.Z.Set( matrix.X.Z, matrix.Y.Z, matrix.Z.Z, matrix.W.Z );
		transpose.W.Set( matrix.X.W, matrix.Y.W, matrix.Z.W, matrix.W.W );
	}

	Matrix4x4 Matrix4x4::Transpose( Matrix4x4% matrix )
	{
		Matrix4x4 transpose;
		transpose.X.Set( matrix.X.X, matrix.Y.X, matrix.Z.X, matrix.W.X );
		transpose.Y.Set( matrix.X.Y, matrix.Y.Y, matrix.Z.Y, matrix.W.Y );
		transpose.Z.Set( matrix.X.Z, matrix.Y.Z, matrix.Z.Z, matrix.W.Z );
		transpose.W.Set( matrix.X.W, matrix.Y.W, matrix.Z.W, matrix.W.W );

		return transpose;
	}

	void Matrix4x4::Transpose()
	{	
		float xy = X.Y;
		float xz = X.Z;
		float xw = X.W;

		float yz = Y.Z;
		float yw = Y.W;

		float zw = Z.W;

		X.Set( X.X, Y.X, Z.X, W.X );
		Y.Set( xy, Y.Y, Z.Y, W.Y );
		Z.Set( xz, yz, Z.Z, W.Z );
		W.Set( xw, yw, zw, W.W );
	}

	Matrix4x4 Matrix4x4::CreateScale( float scale)
	{
		Matrix4x4 identity = Identity;

		identity.X.X *= scale;
		identity.Y.Y *= scale;
		identity.Z.Z *= scale;

		return identity;
	}

	Matrix4x4 Matrix4x4::CreateScale( Vector3 scale )
	{
		Matrix4x4 identity = Identity;

		identity.X.X *= scale.X;
		identity.Y.Y *= scale.Y;
		identity.Z.Z *= scale.Z;

		return identity;
	}

	Matrix4x4 Matrix4x4::CreateTranslation( float translation )
	{
		Matrix4x4 identity = Identity;
		identity.W.Set( translation, translation, translation, 1.0f );
		return identity;
	}

	Matrix4x4 Matrix4x4::CreateTranslation( Vector3 translation )
	{
		Matrix4x4 identity = Identity;
		identity.W.Set( translation.X, translation.Y, translation.Z, 1.0f );
		return identity;
	}

	Matrix4x4 Matrix4x4::CreateRotationAxis( Vector3 axis, float angle )
	{
		if( axis.Length() != 1.0f )
			axis.Normalize();

		Matrix4x4 result;
		float x = axis.X;
		float y = axis.Y;
		float z = axis.Z;
		float cos = static_cast<float>( System::Math::Cos( static_cast<double>( angle ) ) );
		float sin = static_cast<float>( System::Math::Sin( static_cast<double>( angle ) ) );
		float xx = x * x;
		float yy = y * y;
		float zz = z * z;
		float xy = x * y;
		float xz = x * z;
		float yz = y * z;

		result.X.Set(	xx + (cos * (1.0f - xx)),
						(xy - (cos * xy)) + (sin * z),
						(xz - (cos * xz)) - (sin * y),
						0.0f );


		result.Y.Set( (xy - (cos * xy)) - (sin * z),
					  yy + (cos * (1.0f - yy)),
					  (yz - (cos * yz)) + (sin * x),
					  0.0f );

		result.Z.Set( (xz - (cos * xz)) + (sin * y),
					  (yz - (cos * yz)) - (sin * x),
					  zz + (cos * (1.0f - zz)),
					  0.0f );

		result.W.Set( 0.0f, 0.0f, 0.0f, 1.0f );

		return result;
	}

	Matrix4x4 Matrix4x4::CreateRotationX( float angle )
	{
		float cos = static_cast<float>( System::Math::Cos( static_cast<double>( angle ) ) );
		float sin = static_cast<float>( System::Math::Sin( static_cast<double>( angle ) ) );

		Matrix4x4 result;
		result.X.Set( 1.0f, 0.0f, 0.0f, 0.0f );
		result.Y.Set( cos, sin, 0.0f, 0.0f );
		result.Z.Set( 0.0f, -sin, cos, 0.0f );
		result.W.Set( 0.0f, 0.0f, 0.0f, 1.0f );

		return result;
	}

	Matrix4x4 Matrix4x4::CreateRotationY( float angle )
	{
		float cos = static_cast<float>( System::Math::Cos( static_cast<double>( angle ) ) );
		float sin = static_cast<float>( System::Math::Sin( static_cast<double>( angle ) ) );

		Matrix4x4 result;
		result.X.Set( cos, 0.0f, -sin, 0.0f );
		result.Y.Set( 0.0f, 1.0f, 0.0f, 0.0f );
		result.Z.Set( sin, 0.0f, cos, 0.0f );
		result.W.Set( 0.0f, 0.0f, 0.0f, 1.0f );

		return result;
	}

	Matrix4x4 Matrix4x4::CreateRotationZ( float angle )
	{
		float cos = static_cast<float>( System::Math::Cos( static_cast<double>( angle ) ) );
		float sin = static_cast<float>( System::Math::Sin( static_cast<double>( angle ) ) );

		Matrix4x4 result;
		result.X.Set( cos, sin, 0.0f, 0.0f );
		result.Y.Set( -sin, cos, 0.0f, 0.0f );
		result.Z.Set( 0.0f, 0.0f, 1.0f, 0.0f );
		result.W.Set( 0.0f, 0.0f, 0.0f, 1.0f );

		return result;
	}
}
}